﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;

namespace MonoPhysics
{
    /// <summary>
    /// Moteur physique.
    /// </summary>
    public partial class MonoPhysics
    {
        // Vars
        public static List<Edge> cpl;
        private List<RigidBody> moveableScene;
        private List<RigidBody> Ground;
        private List<RigidBody> interactiveBackgroundElements;
        private Queue<RigidBody> _threadSafe_Ground = new Queue<RigidBody>();
        private Queue<RigidBody> _threadSafe_moveable = new Queue<RigidBody>();
        private Queue<RigidBody> _threadSafe_intercative = new Queue<RigidBody>();
        private Queue<RigidBody> _threadSafe_toDelete = new Queue<RigidBody>();

        private bool active;
        private BroadPhase bp;
        private ContactGraph cg = new ContactGraph();
        // For debug
        public bool SweepAndPruneON = true;
        public bool CorrectionsUseMink = true;
        public int NbrIsland = 0;
        public int NbrBroadPhaseResults = 0;
        public bool showCollisionMarks = false;
        // End: For Debug
        public bool Active
        {
            set { active = value; }
            get { return this.active; }
        }

        public MonoPhysics()
        {
            this.active = false;
            MonoPhysics.cpl = new List<Edge>();
            this.moveableScene = new List<RigidBody>();
            this.Ground = new List<RigidBody>();
            this.interactiveBackgroundElements = new List<RigidBody>();
            this.bp = new BroadPhase(delegate(RigidBody r1, RigidBody r2)
                { return (r2.M != 0 || r1.M != 0 || !r1.PhysicStats.V.isNull() || !r2.PhysicStats.V.isNull()); });
        }

        #region Ajout et supression d'elements physiques
        public void AddRigidBody(RigidBody el)
        {
            if (el.M == 0)
            {
                this._threadSafe_Ground.Enqueue(el);
            }
            else
            {
                PhysicContext nPC = el.PhysicStats;
                nPC.LastStablePosDim = el.Pos;
                el.PhysicStats = nPC;
                this._threadSafe_moveable.Enqueue(el);
            }
            if (el.HasInteractiveModeEnabled)
            {
                //if (!interactiveBackgroundElements.Contains(el))
                //    this._threadSafe_intercative.Enqueue(el);
            }
        }
        public void RemoveRigidBody(RigidBody ie)
        {
            this._threadSafe_toDelete.Enqueue(ie);
        }

        private void SafeRemoveBody()
        {
            while (this._threadSafe_toDelete.Count != 0)
            {
                RigidBody ie = this._threadSafe_toDelete.Dequeue();
                bp.DeleteItemInBroadPhase(ie);
                cg.DeleteNode(ie);
                if (moveableScene.Contains(ie))
                    moveableScene.Remove(ie);
                else if (Ground.Contains(ie))
                    Ground.Remove(ie);
                if (ie.HasInteractiveModeEnabled)
                {
                    if (interactiveBackgroundElements.Contains(ie))
                        interactiveBackgroundElements.Remove(ie);
                }
            }
        }

        private void SafeAddBodies()
        {
            while (this._threadSafe_Ground.Count != 0)
            {
                RigidBody el = this._threadSafe_Ground.Dequeue();
                if (!this.Ground.Contains(el))
                {
                    el.IncrementalUpdateEndpoints(this.bp);
                    bp.AddItemToBroadPhase(el);
                    this.Ground.Add(el);
                }
            }
            while (this._threadSafe_intercative.Count != 0)
            {
                RigidBody el = this._threadSafe_intercative.Dequeue();
                if (!this.interactiveBackgroundElements.Contains(el))
                {
                    el.IncrementalUpdateEndpoints(this.bp);
                    bp.AddItemToBroadPhase(el);
                    this.interactiveBackgroundElements.Add(el);
                }
            }
            while (this._threadSafe_moveable.Count != 0)
            {
                RigidBody el = this._threadSafe_moveable.Dequeue();
                if (!this.moveableScene.Contains(el))
                {
                    el.IncrementalUpdateEndpoints(this.bp);
                    el.InitArbiter(50, 2);
                    bp.AddItemToBroadPhase(el);
                    this.moveableScene.Add(el);
                }
            }

        }

        public void ClearPhysics()
        {
            lock (moveableScene)
            {
                lock (Ground)
                {
                    lock (interactiveBackgroundElements)
                    {
                        cpl.Clear();
                        moveableScene.Clear();
                        interactiveBackgroundElements.Clear();
                        Ground.Clear();
                    }
                }
            }
        }
        #endregion

        public bool MayPhysicallyCollude(RigidBody r1, RigidBody r2, bool useCentroid)
        {
            Edge e = new Edge();
            e.ue1 = r1;
            e.ue2 = r2;
            return (NarrowPhase.Rule(e) && NarrowPhase.Hascontacts(e, useCentroid));
        }

        public RigidBody MayPhycicallyColudeWithAny(RigidBody r, float delta, bool useCentroid)
        {
            foreach (RigidBody rb in this.moveableScene)
            {
                Edge e = new Edge();
                e.ue2 = rb;
                e.ue1 = r;
                if (!NarrowPhase.Rule(e))
                    continue;
                else if (NarrowPhase.Hascontacts(e, delta, useCentroid) || (rb.logical))
                    return rb;
            }
            foreach (RigidBody rb in this.Ground)
            {
                Edge e = new Edge();
                e.ue2 = rb;
                e.ue1 = r;
                if (!NarrowPhase.Rule(e) || (rb.logical))
                    continue;
                else if (NarrowPhase.Hascontacts(e, delta, useCentroid))
                    return rb;
            }
            return null;
        }


        public void UpdatePhysics(float dt)
        {
            if (!active)
                return;
            try
            {
                //this.cg.ReinitDamagedNodes(); // il faut reinitialiser les branches avant d'en en suprimer eventuellement (appel a SafeAddBody)
                this.SafeRemoveBody();
                this.SafeAddBodies();
                // On met a jour tous les objets.
                //Universe u = Monochrome.DataEngine.Serializer.SerializableObjects.universe.LoadedUniverse;
                if (dt > 0.016f)
                    dt = 0.016f;
                // 1) On update les position (On regarde la vitesse et bouje l'objet en fonction.

                foreach (RigidBody ce in moveableScene)
                {
                    ce.SolveArbiter();
                    if (!ce.Sleeping && !ce.PhysicStats.IgnoreGravity)
                        ImpulseSolver.applyGravity(ce);
                }
                foreach (RigidBody ce in this.moveableScene)
                {
                    if (!ce.Sleeping)
                        ImpulseSolver.ExecObjectforces(ce, dt);
                }
                List<RigidBody> movedObjects = ImpulseSolver.GetLasMovedObjects();
                foreach (RigidBody r in movedObjects)
                { r.IncrementalUpdateEndpoints(this.bp); }

                ImpulseSolver.ClearLasMovedObjects();

                List<Edge> D = bp.GetBroadPhaseResult(dt);
                if (D != null)
                {

                    this.NbrBroadPhaseResults = D.Count;
                    cg.AddEdges(D);
                }

                foreach (RigidBody ce in moveableScene)
                {
                    ce.CollisionPoints.Clear();
                    ce.contactTgts.Clear(); // on en profite pour clear les tengeantes!
                }

                this.cg.ApplyNarrowPhase(this);
                List<ContactGraph> islands = this.cg.BuildContactGroups();
                this.NbrIsland = islands.Count;
                foreach (ContactGraph gr in islands)
                    gr.Solve(dt);
                // 2.4) On actionne d'eventuels objets actifs en cas de collisions.
                this.cg.GenerateEvents();
                this.cg.ReplaceObjects();
                //this.cg.SetToTOIStep(dt);
                //Console.WriteLine("1 = " + (t2 - t1) + "  /////  Total = " + (Timer.TicksElapsed - t1).ToString() + "   ///// Reste = " + ((Timer.TicksElapsed - t1) - (t2 - t1)).ToString());
                // La suite part a l'affichage.

            }
            catch { }//Console.WriteLine("ERREUR"); }
        }
    }
}
